% vivado project QPSK_IMPLEMENTATION_USING_AD9361
clc;
clear;
close all;

% ------------------------------Transmitter-----------------------------------------
%% Params Init
Fs = 10e6; %sample frequency
Fd = 1e3;
Bd = 2.5e6; %baud rate
M = 4; %QPSK
OSR = Fs / Bd; %samples per symbol
EBN0 = 11;
SNR = EBN0 + 10 * log10(2 / OSR);
ROLLOF = 0.5; %roll-off factor for rcosdesign func

%% data generate
data = 1 * exp(sqrt(-1) * 2 * pi * randi([0 M - 1], 1, 10000) / M);

%% upsample, shaping filter, farrow interpolation
% 4x upsample and shaping filter using upfirdn
rcos_fir = round(1.15 * rcosdesign(ROLLOF, 6, Fs / Bd, 'sqrt') * 2^15);
% 12+0 * 1+15 -> 1+11
data_rcos = upfirdn(data, rcos_fir, OSR) / 16;
% farrow interpolation using farrow func
data_tx = floor(farrow_int(Fs, 60e6, data_rcos));
clear data_rcos

%% modulation and channel transmission
signal_carrier = exp(sqrt(-1) * 2 * pi * (0:length(data_tx) - 1) / 60e6 * Fd)';
signal_rx = awgn(signal_carrier .* data_tx, SNR, 'measured');
signal_rx = floor(signal_rx / max(abs(signal_rx)) * 2^11);

%% ------------------------------Receiver-----------------------------------------
%% demodulation, downsample and matched filter
signal_rx = downsample([signal_rx], 6);
signal_rx = floor(filter(rcos_fir, 1, signal_rx) / (2^16));

%% Carrier Synchronization and Bit Synchronization
% using PLL and Gardner Loop
GardnerParams = GardnerInit();
PllParams = PllInit();

for index_data = 1:length(signal_rx)
    % calculation Gardner nco
    GardnerParams.nco(2) = GardnerParams.nco(1) - GardnerParams.w_fifo(1);
    % pll demodulation
    %     PllParams.phase = mod(PllParams.phase + PllParams.FTW,2^32);
    PllParams.phase = PllParams.phase + PllParams.FTW(1);

    input = signal_rx(index_data) * exp(-1 * sqrt(-1) * 2 * pi * PllParams.phase / (2^32));
    % pll error calculation
    PllParams.phase_error = -imag(input) * sign(real(input)) + real(input) * sign(imag(input));
    % pll loop filter
    PllParams.loopfilter = PllParams.loopfilter + PllParams.phase_error;
    PllParams.FTW = [PllParams.FTW(2:end); floor(PllParams.loopfilter * 2^ - 2) + floor(PllParams.phase_error * 2^10)];

    PllParams.watch.phase = [PllParams.watch.phase, PllParams.phase];
    PllParams.watch.phase_error = [PllParams.watch.phase_error, PllParams.phase_error];
    PllParams.watch.loopfilter = [PllParams.watch.loopfilter, PllParams.loopfilter];
    PllParams.watch.FTW = [PllParams.watch.FTW, PllParams.FTW];

    % load pll demodulation data for farrow interpolation
    GardnerParams.data_for_farrow = [input; GardnerParams.data_for_farrow(1:end - 1)];
    % Gardner farrow interpolation
    FI1 = floor(0.5 * GardnerParams.data_for_farrow(1)) - floor(0.5 * GardnerParams.data_for_farrow(2)) ...
    - floor(0.5 * GardnerParams.data_for_farrow(3)) + floor(0.5 * GardnerParams.data_for_farrow(4));
    FI2 = GardnerParams.data_for_farrow(2) + floor(0.5 * GardnerParams.data_for_farrow(2)) ...
        - floor(0.5 * GardnerParams.data_for_farrow(1)) - floor(0.5 * GardnerParams.data_for_farrow(3)) ...
        - floor(0.5 * GardnerParams.data_for_farrow(4));
    FI3 = GardnerParams.data_for_farrow(3);
    temp1 = floor(FI1 * GardnerParams.u / (2^15));
    temp2 = floor(temp1 * GardnerParams.u / (2^15));
    temp3 = floor(FI2 * GardnerParams.u / (2^15));

    % error calculation should be implemented if nco overflow
    if (GardnerParams.nco(2) <= 0)

        % load farrow interpolation result into farrow_output array
        GardnerParams.farrow_output = [temp2 + temp3 + FI3; GardnerParams.farrow_output(1:end - 1)];
        GardnerParams.farrow_num = GardnerParams.farrow_num + 1;

        % add farrow output to GardnerParams.watch
        GardnerParams.watch.farrow_output = [GardnerParams.watch.farrow_output; GardnerParams.farrow_output(1)];

        % error calculation
        if (mod(GardnerParams.farrow_num, 2) == 0)
            err_real = real(GardnerParams.farrow_output(1)) - real(GardnerParams.farrow_output(3));
            err_imag = imag(GardnerParams.farrow_output(1)) - imag(GardnerParams.farrow_output(3));
            err_mul_real = real(GardnerParams.farrow_output(2)) * err_real;
            err_mul_imag = imag(GardnerParams.farrow_output(2)) * err_imag;

            GardnerParams.time_error = [err_mul_real + err_mul_imag; GardnerParams.time_error(1)];
            clear err_real err_imag err_mul_real err_mul_imag;

            % second order loop filter
            GardnerParams.err_filter = GardnerParams.err_filter + GardnerParams.time_error(1);
            GardnerParams.w_delta = floor(GardnerParams.err_filter / (2^18)) + floor(GardnerParams.time_error(1) / (2^11));
            GardnerParams.w = 16370 + GardnerParams.w_delta;
            GardnerParams.w_fifo = [GardnerParams.w_fifo(2:end); GardnerParams.w];

            % add error and w to watch
            GardnerParams.watch.time_error = [GardnerParams.watch.time_error GardnerParams.time_error];
            GardnerParams.watch.w = [GardnerParams.watch.w GardnerParams.w];
            GardnerParams.watch.w_delta = [GardnerParams.watch.w_delta GardnerParams.w_delta];
            GardnerParams.watch.err_filter = [GardnerParams.watch.err_filter, GardnerParams.err_filter];
        end

        % update u
        GardnerParams.u = floor(GardnerParams.nco(1) * 2);
        GardnerParams.watch.u = [GardnerParams.watch.u GardnerParams.u];

        GardnerParams.nco(2) = mod(GardnerParams.nco(2), 2^15);
    end

    GardnerParams.nco(1) = GardnerParams.nco(2);
    GardnerParams.watch.nco = [GardnerParams.watch.nco GardnerParams.nco(2)];
end

%% ------------------------------Debug-----------------------------------------
GardnerParams.output = GardnerParams.watch.farrow_output(2:2:end);
figure;
subplot(121); plot(GardnerParams.output(1000:end), '*');
subplot(122); plot(real(GardnerParams.watch.farrow_output));
hold on; plot(real(GardnerParams.output), 'r*');
figure; plot(PllParams.watch.phase); title('phase');
figure; plot(PllParams.watch.loopfilter); title('loopfilter');
